#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     gyro,           sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3,     light,          sensorLightActive)
#pragma config(Sensor, S4,     sonar,          sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     right,         tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     left,          tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     armMotor,      tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     intakeMotor,   tmotorTetrix, openLoop, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma debuggerWindows("joystickSimple");

#include "pid_includes.h"

task main()
{
	InitializePIDControl();
	StartTask(Controller);

	RequestLeftMotorSpeed(50);
	RequestRightMotorSpeed(50);
	wait1Msec(5000);

	RequestLeftMotorSpeed(100);
	RequestRightMotorSpeed(100);
	wait1Msec(5000);

	RequestLeftMotorSpeed(25);
	RequestRightMotorSpeed(25);
	wait1Msec(5000);

	while(true)
	{
		// Only loop if manual control got turned on //
		if(!ManualControlEnabled())
			break;
	}

}
